from mpython import *
from machine import Pin
import machine
import time
class nezha(object):

 
    def __init__(self,i2c):
        self._i2c = i2c
        self._addr = 0x10

        
    def set_motor_speed(self,motor,speed):
        if (motor == "M1"):
            motor_addr = 0x01
        elif (motor == "M2"):
            motor_addr = 0x02
        elif (motor == "M3"):
            motor_addr = 0x03
        else:
            motor_addr = 0x04
        if (speed < 0):
            speed = speed * -1
            self._i2c.writeto(self._addr, (bytearray([motor_addr,0x02,speed,0])))            
        else:         
            self._i2c.writeto(self._addr, (bytearray([motor_addr,0x01,speed,0])))


     
    def stop_motor(self,motor):
        if (motor == "M1"):
            motor_addr = 0x01
        elif (motor == "M2"):
            motor_addr = 0x02
        elif (motor == "M3"):
            motor_addr = 0x03
        else:
            motor_addr = 0x04
        self._i2c.writeto(self._addr, (bytearray([motor_addr,0x01,0,0])))
        
        

    def stop_all_motor(self):
        self._i2c.writeto(self._addr, (bytearray([0x01, 0x01, 0, 0])))
        self._i2c.writeto(self._addr, (bytearray([0x02, 0x01, 0, 0])))
        self._i2c.writeto(self._addr, (bytearray([0x03, 0x01, 0, 0])))
        self._i2c.writeto(self._addr, (bytearray([0x04, 0x01, 0, 0])))



    def set_servo_angle(self, servotype, servo, angle):
        if servotype == 180:
            if angle > 180:
                angle = 180
            if angle < 0:
                angle = 0                
            angle = round(numberMap(angle, 0, 180, 0, 180))
        elif servotype == 270:
            if angle > 225:
                angle = 225
            if angle < 0:
                angle = 0  
            angle = round(numberMap(angle, -45, 225, 0, 180))
        else:
            if angle > 360:
                angle = 360
            if angle < 0:
                angle = 0  
            angle = round(numberMap(angle, 0, 360, 0, 180))
        if servo == 0:
            servo_addr = 0x10
        elif servo == 1:
            servo_addr = 0x11
        elif servo == 2:
            servo_addr = 0x12
        elif servo == 3:
            servo_addr = 0x13
        self._i2c.writeto(self._addr, (bytearray([servo_addr, angle, 0, 0])))
    
    
    
    def set_servo_speed(self, servo, speed):
        speed = round(numberMap(speed, -100, 100, 0, 180))
        if servo == 0:
            servo_addr = 0x10
        elif servo == 1:
            servo_addr = 0x11
        elif servo == 2:
            servo_addr = 0x12
        elif servo == 3:
            servo_addr = 0x13
        self._i2c.writeto(self._addr, (bytearray([servo_addr, speed, 0, 0])))